﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using WebCam.Data.WebCamWorker;
using System.Drawing;
using System.Drawing.Imaging;

namespace WebCam.Logic.Detectors
{
    public class MotionCaptureEventArgs : WorkerComletedEventArgs
    {        
        public MotionCaptureEventArgs(Bitmap bmp, double leftAngle, double rightAngle)
            :base(bmp)
        {
            LeftAngle = leftAngle;
            RightAngle = rightAngle;
        }

        public double LeftAngle { get; private set; }
        public double RightAngle { get; private set; }
    }
    public class MotionCaptureWorker : IWorker
    {
        private Bitmap oldImage = null;
        private Rectangle rect;
        private int delta = 70;
        private int step = 2;
        private double oldLeftAngle = 0;
        private double oldRightAngle = 0;

        public void ProcessBitmap(Bitmap image)
        {
            if(oldImage == null)
            {
                oldImage = (Bitmap)image.Clone();
                return;
            }

            rect = new Rectangle(0,0,image.Width, image.Height);

            Bitmap tmp = (Bitmap)image.Clone();

            BitmapData imageData = image.LockBits(rect, ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
            BitmapData oldImageData = oldImage.LockBits(rect, ImageLockMode.ReadOnly, PixelFormat.Format24bppRgb);
            BitmapData tmpData = tmp.LockBits(rect, ImageLockMode.WriteOnly, PixelFormat.Format24bppRgb);

            List<Point> leftDiff = new List<Point>();
            List<Point> rightDiff = new List<Point>();

            unsafe
            {
                byte* pImage = (byte*)imageData.Scan0.ToPointer();
                byte* pOldImage = (byte*)oldImageData.Scan0.ToPointer();
                byte* pTmpImage = (byte*)tmpData.Scan0.ToPointer();

                //int index = 0;
                for (int j = 0; j < image.Height; j += step)
                {
                    for (int i = 0; i < image.Width; i += step)
                    {
                        byte r = *pImage;
                        byte g = *(pImage + 1);
                        byte b = *(pImage + 2);

                        byte or = *(pOldImage);
                        byte og = *(pOldImage + 1);
                        byte ob = *(pOldImage + 2);

                        if (Math.Abs(r - or) > delta || Math.Abs(g - og) > delta || Math.Abs(b - ob) > delta)
                        {
                            *pTmpImage = 0;
                            *(pTmpImage + 1) = 0;
                            *(pTmpImage + 2) = 255;
                            if (i < image.Width / 2)
                            {
                                leftDiff.Add(new Point(i, j));
                            }
                            if (i > image.Width / 2)
                            {
                                rightDiff.Add(new Point(i, j));
                            }
                        }

                        pImage += 3 * step;
                        pOldImage += 3 * step;
                        pTmpImage += 3 * step;
                        //index++;
                    }
                    pImage += 3 * image.Width * (step - 1);
                    pOldImage += 3 * image.Width * (step - 1);
                    pTmpImage += 3 * image.Width * (step - 1);
                }
            }

            image.UnlockBits(imageData);
            tmp.UnlockBits(tmpData);
            oldImage.UnlockBits(oldImageData);

            oldImage = (Bitmap)image.Clone();

            double leftAngle = 0, rightAngle = 0;
            int leftCount = 0, rightCount = 0;
            Point center = new Point(image.Width / 2, image.Height);

            // пороговое ограничение на число обрабатываемых точек
            if (leftDiff.Count > 100)
            {
                // пробегаемся по всем найденным точкам
                for (int i = 0; i < leftDiff.Count; i++)
                {
                    // если угол наклона для данной точки сильно отличается от искомого, то считаем ее помехой
                    // иначе добавляем ее к среднему углу
                    double angle = GetAngle(leftDiff[i], center);
                    if (oldLeftAngle == 0 || Math.Abs(angle - oldLeftAngle) < 0.5)
                    {
                        leftAngle += angle;
                        leftCount++;
                    }
                }
            }
            // пороговое ограничение на число обрабатываемых точек
            if (rightDiff.Count > 100)
            {
                for (int i = 0; i < rightDiff.Count; i++)
                {
                    // если угол наклона для данной точки сильно отличается от искомого, то считаем ее помехой
                    // иначе добавляем ее к среднему углу
                    
                    double angle = GetAngle(rightDiff[i], center);

                    if (oldRightAngle == 0 || Math.Abs(angle - oldRightAngle) < 0.5)
                    {
                        rightAngle += angle;
                        rightCount++;
                    }
                }
            }

            leftAngle = (leftCount == 0 ? 0 : leftAngle / leftCount);
            rightAngle = (rightCount == 0 ? 0 : rightAngle / rightCount);

            if (leftAngle == 0)
            {
                leftAngle = oldLeftAngle;
            }
            if (rightAngle == 0)
            {
                rightAngle = oldRightAngle;
            }

            oldLeftAngle = leftAngle;
            oldRightAngle = rightAngle;

            if (OnWorkerComplete != null)
            {
                OnWorkerComplete(this, new MotionCaptureEventArgs(tmp, leftAngle, rightAngle));
            }
        }

        double GetAngle(Point first, Point second)
        {
            if (second.X != first.X)
            {
                return Math.Atan(((double)first.Y - second.Y) / 
                    (first.X - second.X));
            }
            else if (second.Y != first.Y)  // считаем по Y
            {
                return 1 / Math.Atan(((double)first.X - second.X) / 
(first.Y - second.Y));
            }
            else // на самый крайний случай, если точки совпадают
            {
                return 0;
            }
        }

        public event WorkerComplete OnWorkerComplete;
    }
}
